#!/usr/bin/python3
# coding=utf8
import sys
import time
import signal
sys.path.append('/home/pi/MasterPi/')
import HiwonderSDK.Board as Board
from Common import chassis

if sys.version_info.major == 2:
    print('Please run this program with python3!')
    sys.exit(0)

def Forward():
    chassis.set_velocity(50,90,0)
def Left():
    chassis.set_velocity(50,180,0)
def Back():
    chassis.set_velocity(50,270,0)
def Right():
    chassis.set_velocity(50,360,0)
    


def turn_left():
    chassis.set_velocity(0,0,-0.3)
def turn_right():
    chassis.set_velocity(0,0,0.3)

def Stop():
    chassis.set_velocity(0, 0, 0)


# pos为500到2500的值，对应角度-90到90的映射
# 从上到下五个舵机编号分别为1，3，4，5，6
def set_first_Servo(time,pos):
    Board.setPWMServosPulse([time,1,1,pos])
def set_second_Servo(time,pos):
    Board.setPWMServosPulse([time,1,3,pos])
def set_third_Servo(time,pos):
    Board.setPWMServosPulse([time,1,4,pos])
def set_fourth_Servo(time,pos):
    Board.setPWMServosPulse([time,1,5,pos])
def set_fifth_Servo(time,pos):
    Board.setPWMServosPulse([time,1,6,pos])

# 立正
def arm_up():
    set_first_Servo(1000,1500)
    set_second_Servo(1000,1500)
    set_third_Servo(1000,1500)
    set_fourth_Servo(1000,1500)
    set_fifth_Servo(1000,1500)
    time.sleep(0.3)
    set_first_Servo(500,2500)
    time.sleep(0.3)
    set_first_Servo(500,1500)
    time.sleep(0.3)
    set_first_Servo(500,2500)

# 左右摆头
def arm_both():
    set_first_Servo(1000,1500)
    set_second_Servo(1000,700)
    set_third_Servo(1000,2200)
    set_fourth_Servo(1000,1000)
    set_fifth_Servo(1000,2000)
    time.sleep(0.5)
    set_fifth_Servo(1000,1000)
    time.sleep(0.5)
    set_fifth_Servo(1000,1500)
    time.sleep(0.5)
    set_fifth_Servo(1000,2000)
    time.sleep(0.5)
    set_fifth_Servo(1000,1000)
    time.sleep(0.5)
    set_fifth_Servo(1000,1500)



# 处理 Ctrl+C 终止信号
def signal_handler(sig, frame):
    Stop()
    print('stop')
    sys.exit(0)

signal.signal(signal.SIGINT, signal_handler)

if __name__ == '__main__':
    while True:
        Forward()
        time.sleep(1)
